import cv2
import cv2.aruco as aruco
import numpy as np

# 初始化摄像头
cap = cv2.VideoCapture(1)  # 0表示默认摄像头，如果有多个摄像头可以尝试1,2等

# 设置Aruco字典
aruco_dict = aruco.getPredefinedDictionary(aruco.DICT_4X4_1000)
parameters = aruco.DetectorParameters()

# 创建检测器
detector = aruco.ArucoDetector(aruco_dict, parameters)

# 要检测的特定Marker ID
TARGET_MARKER_ID = 1000

while True:
    # 读取摄像头帧
    ret, frame = cap.read()
    if not ret:
        print("无法获取视频帧")
        break

    # 转换为灰度图像
    gray = cv2.cvtColor(frame, cv2.COLOR_BGR2GRAY)

    # 检测Markers
    corners, ids, rejected = detector.detectMarkers(gray)

    # 如果检测到任何Marker
    if ids is not None:
        # 绘制所有检测到的Markers
        frame = aruco.drawDetectedMarkers(frame, corners, ids)

        # 检查是否有我们感兴趣的特定Marker (ID=1000)
        # if TARGET_MARKER_ID in ids:
        #     # 找到该Marker的索引
        #     idx = np.where(ids == TARGET_MARKER_ID)[0][0]
        #
        #     # 获取该Marker的角点
        #     target_corners = corners[idx][0]
        #
        #     # 计算中心点
        #     center = target_corners.mean(axis=0)
        #
        #     # 在中心点绘制一个圆
        #     cv2.circle(frame, tuple(center.astype(int)), 10, (0, 255, 0), -1)
        #
        #     # 显示ID和位置信息
        #     cv2.putText(frame, f"ID {TARGET_MARKER_ID} detected",
        #                 (10, 30), cv2.FONT_HERSHEY_SIMPLEX,
        #                 1, (0, 255, 0), 2)

    # 显示结果
    cv2.imshow('Aruco Marker Detection', frame)

    # 按'q'退出
    if cv2.waitKey(1) & 0xFF == ord('q'):
        break

# 释放资源
cap.release()
cv2.destroyAllWindows()